import numpy as np
import os
import trimesh


#def angle_calculate(point, normal, endpoint):
#    midpoint = (endpoint[0] + endpoint[1]) / 2
#    direction = midpoint - point 
#    direction = direction / np.linalg.norm(direction)
#    
#    value = np.abs(np.dot(normal,direction))
#    if value > 1:
#        value = 1
#    angle = np.arccos(value) / np.pi
#    return angle

#def discrete_mean_curvature_measure(mesh, points, normals, radius): 
#    #G = build_graph(mesh)
#    points = np.asanyarray(points, dtype=np.float64)
##    if not util.is_shape(points, (-1, 3)):
##        raise ValueError('points must be (n,3)!')
#
#    # axis aligned bounds
#    bounds = np.column_stack((points - radius,
#                              points + radius))
#    # line segments that intersect axis aligned bounding box
#    candidates = [list(mesh.face_adjacency_tree.intersection(b))
#                  for b in bounds]
#
#    mean_curv = np.empty(len(points))
#    for i, (x, x_normal, x_candidates) in enumerate(zip(points, normals, candidates)):
#        endpoints = mesh.vertices[mesh.face_adjacency_edges[x_candidates]]
#        angle =np.array([angle_calculate(x, x_normal, endpoint)for endpoint in endpoints])
##        signed_distance =trimesh.proximity.signed_distance(mesh,[(x + (endpoint[0] + endpoint[1])/2)/2 for endpoint in endpoints])
##        #print(signed_distance)
##        #dist_ = [nx.shortest_path_length(G, source=0, target=idx, weight='length') for idx in value[:, 1]]
##        #dist_mask = np.array(dist_) < 0.01
##        #
##        #mask_ = signed_distance < 0.003
##        #mask = dist_mask & dist_mask
##        mask = signed_distance < 0.002
##        
##        x_candidates = [d for d, s in zip(x_candidates, mask) if s]
#        mask = angle < 2/3
#        
#        x_candidates = [d for d, s in zip(x_candidates, mask) if s]
#        endpoints = mesh.vertices[mesh.face_adjacency_edges[x_candidates]]
#
#        lengths = line_ball_intersection(
#            endpoints[:, 0],
#            endpoints[:, 1],
#            center=x,
#            radius=radius)
#        angles = mesh.face_adjacency_angles[x_candidates]
#        signs = np.where(mesh.face_adjacency_convex[x_candidates], 1, -1)
#        mean_curv[i] = (lengths * angles * signs).sum() / 2
#
#    return mean_curv

if __name__ == "__main__":
    begin = 0
    i = 0
    import json
    with open('/home/v-wewei/finish_stl/dataset_dict.json', 'r') as f:
        dataset_dict = json.load(fp = f)
        for key in dataset_dict.keys():

            if key != '073-d_lego_duplo':
               continue
                
            path = dataset_dict[key]['path']
            print(path)
            mesh = trimesh.load_mesh(path)
            trimesh.smoothing.filter_humphrey(mesh)
            
            cur_path = path.replace('.stl', '.npy')
           # MC = np.load(cur_path)
            MC = np.load('/home/v-wewei/finish_stl/{}.npy'.format(key))
            threshold = 80
            point_positive = []
            point_negtive = []
            for idx in range(len(mesh.vertices)):
                
                if  -threshold   < MC[idx] < threshold:
                    point_positive.append(mesh.vertices[idx])
                else:
                    neighbors = mesh.vertex_neighbors[idx]
                    mask = (-threshold  > MC[neighbors]).any() or (threshold < MC[neighbors]).any()
                    if mask: 
                        point_negtive.append(mesh.vertices[idx])
                    else:
                        point_positive.append(mesh.vertices[idx])
                #print('idx is : ', idx)
                #print(MC[idx])
                #MC[idx] = MC[mesh.vertex_neighbors[idx]].mean()
                #print(MC[idx])
                #print(MC[mesh.vertex_neighbors[idx]])
            print('negtive points num is : ', len(point_negtive))
            #if len(point_negtive) == 0:
            #    print('----------------', key, '----------------')
            #np.save(cur_path, MC)
            debug_vis = True
            if debug_vis:

                visual_content = []
                if len(point_positive) > 0:
                    pointcloud_positive = trimesh.PointCloud(np.asarray(point_positive).reshape(-1, 3), colors=[0, 255, 0])
                    visual_content.append(pointcloud_positive)
                if len(point_negtive) > 0:
                    pointcloud_negtive = trimesh.PointCloud(np.asarray(point_negtive).reshape(-1, 3), colors=[255, 0, 0])
                    visual_content.append(pointcloud_negtive)
                mesh.visual.face_colors = [255, 255, 255]
                scene = trimesh.Scene([mesh, visual_content])
                scene.show()

